/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    StateEstimator.h
  * @author  Mentos Seetoo
  * @brief   //TODO 状态估计器是用于估计机器人在物理世界中的状态，包括质心的速度、高度和
  *          姿态，腿部、身体与物理环境的交互状态及中间变量等。
  * @note    THe following third-party libraries are used:
  *          - Eigen3
  * @warning 
  *     - At least C++11 is required.												 
  ******************************************************************************
  */
#ifndef _STATE_ESTIMATOR_H
#define _STATE_ESTIMATOR_H
/* Includes ------------------------------------------------------------------*/
#include <vector>
#include "Utilities/additionTypes.h"

namespace Quadruped
{

template <typename T>
class StateEstimator
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    //!< CoM state in {G}
    Vec3<T> p_raw, v_raw, w_raw, rpy_raw;
    Vec3<T> p, v, w, rpy;                 

    //!< Leg state
    Vec3<T> p_foot_M[4];    //!< Foot location in {M}.
    Vec3<T> p_foot_G[4];    //!< Foot location in {G}.
    Vec3<T> p_initFoot[4];  //!< Init foot location
    StateEstimator();
};

} // namespace Quadruped
#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/